#include "StdAfx.h"
#include "MathUtil.h"

#define PI 3.14159265

float GetAngle(CvPoint refPoint, CvPoint pt1);
MathUtil::MathUtil(void)
{
}

MathUtil::~MathUtil(void)
{
}

float MathUtil::GetDistance(CvPoint pt1, CvPoint pt2)
{
	return (float)sqrt((double)((pt1.x - pt2.x) * (pt1.x - pt2.x) + (pt1.y - pt2.y) * (pt1.y - pt2.y)));
}

CvPoint MathUtil::GetTranslatedPoint(CvPoint orginalReferencePoint, 
									 CvPoint originalPointOne, 
									 CvPoint originalPointTwo, 
									 CvPoint transformedPointOne, 
									 CvPoint transformedPointTwo)
{
	float rotationAngle = GetAngle(transformedPointOne, transformedPointTwo) - GetAngle(orginalReferencePoint, originalPointOne);
	int x = originalPointTwo.x - orginalReferencePoint.x;
	int y = originalPointTwo.y - orginalReferencePoint.y;
	int newX = x * cos(rotationAngle) - y * sin(rotationAngle) + transformedPointOne.x;
	int newY = x * sin(rotationAngle) + y * cos(rotationAngle) + transformedPointOne.y;

	return cvPoint(newX, newY);
}

float GetAngle(CvPoint refPoint, CvPoint pt1)
{
	int x = pt1.x - refPoint.x;
	int y = pt1.y - refPoint.y;

	float angleInRadiens = 0;
	if (x == 0)
	{
		angleInRadiens = PI / 2;
	}
	else
	{
		angleInRadiens = atan((double)y / x);
	}

	if (x > 0 && y > 0)
	{
		//do nothing
	}
	else if (x < 0 && y > 0)
	{
		angleInRadiens += PI;
	}
	else if  (x < 0 && y < 0)
	{
		angleInRadiens += PI;
	}
	else 
	{
		angleInRadiens += 2 * PI;
	}

	return angleInRadiens;
}